import java.io.PrintStream;

import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.addon.IRSeeker;
import lejos.robotics.subsumption.Behavior;

//############################################
// Handles ball locating and move towards it #
//############################################
/**
 * @author Mitchell
 * 
 */
public class FindBallAndMoveToward implements Behavior {

	// Value of the turn for the wheels per IR step
	private int turnValue = 20;

	// Supsumtion Supression logic
	private boolean suppressed = false;

	// set ball direction to non visual
	private int ballDirection = 255;
	private int irSensor1;
	private int irSensor2;
	private int irSensor3;
	private int irSensor4;
	private int irSensor5;

	public void suppress() {
		suppressed = true;
	}

	private Boolean BallIsFound() {

		ballDirection = BillManager.GetInfraredData();
		irSensor1 = BillManager.GetInfraredSensor1Data();
		irSensor2 = BillManager.GetInfraredSensor2Data();
		irSensor3 = BillManager.GetInfraredSensor3Data();
		irSensor4 = BillManager.GetInfraredSensor4Data();
		irSensor5 = BillManager.GetInfraredSensor5Data();

		// Ball is found
		if (ballDirection != 0) {
			return true;
		} else {
			return false;
		}
	}

	public void action() {
		suppressed = false;

		int accA = Motor.A.getAcceleration();
		int accB = Motor.B.getAcceleration();
		int speedA = Motor.A.getSpeed();
		int speedB = Motor.B.getSpeed();

		// ball is located to the left of the robot
		if (ballDirection >= 1 && ballDirection < 5 && irSensor3 < 130) {
			int difference = 5 - ballDirection;
			Motor.A.setSpeed(60);
			Motor.B.setSpeed(60);
			//Motor.A.setAcceleration(5);
			//Motor.B.setAcceleration(5);
			Motor.A.backward();
			Motor.B.forward();
			//Motor.A.rotate(-(difference * turnValue), true);
			//Motor.B.rotate((difference * turnValue), true);
		}

		// ball is located to the right of the robot
		else if (ballDirection > 5 && ballDirection <= 9 && irSensor3 < 130) {
			int difference = ballDirection - 5;
			Motor.A.setSpeed(60);
			Motor.B.setSpeed(60);
			//Motor.A.setAcceleration(5);
			//Motor.B.setAcceleration(5);
			Motor.A.forward();
			Motor.B.backward();
			//Motor.A.rotate((difference * turnValue), true);
			//Motor.B.rotate(-(difference * turnValue), true);
		}

		// ball is located in a direct line of the robot - move forward a
		// bit and check again to have persistent data
		else if (ballDirection == 5 && irSensor3 < 130) {
			Motor.A.forward();
			Motor.B.forward();
		} else if (irSensor3 > 130) {
			BillManager.SetHasBall(true);
		}

		while (BallIsFound() && !suppressed)
			Thread.yield();

		Motor.A.stop();
		Motor.B.stop();
		
		Motor.A.setSpeed(speedA);
		Motor.B.setSpeed(speedB);
		//Motor.A.setAcceleration(accA);
		//Motor.B.setAcceleration(accB);
	}

	public boolean takeControl() {
		return BallIsFound();
	}
}
